#ifndef _ROBOTCONTROL_H_
#define _ROBOTCONTROL_H_

#include "kinematics.h"
#include "PCA9685_servo.h"
#include "Config.h"

/*
    robot body:
    ----o-----2******1----o-----
              *      *   
              *      *
              *      *
    ----o-----4******3----o-----
*/

/*
    leg:
        joint1 o----o joint2
                     \
                      \
                       \
*/
#define SWING   0
#define SUPPORT 1

typedef struct {
    float body_width;
    float body_length;
    DH    dh_params;
}RobotConfig;

typedef enum{
    Walk = 0, Run, Turn, Stop,
} MOVEMODE;

void robotController_init();
void move_to_home();
void move_leg(u8 leg_channel, JointAngle* jointangle, u16 cost_time);

#endif